/**
  ******************************************************************************
  * @file    test_matplot.cpp
  * @author  Mentos Seetoo
  * @brief   This file provides examples to use matplotlib-cpp and test whether
  *          this library can work well in your enviroment.		
  * @warning 
  *          - At least C++11 is required.														 
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "test.h"

using namespace Quadruped;
namespace plt = matplotlibcpp;

GaitScheduler<float> Gait_Tester;
DesiredCoMState<float> Cmd_Tester(0.005);
/* Private defines -----------------------------------------------------------*/
static void test_2D_traj();
static void test_3D_traj();

/*
    @brief test function, good to work but it would cause bad line.
*/
template<typename T>
struct cuboidSize{ T xmin, xmax, ymin, ymax, zmin, zmax;};

template<typename T>
void plot_3D_cuboidFrame(const T &x, const T &y, const T &z,
                        const struct cuboidSize<T> &size,
                        const std::map<std::string, std::string> &kw =
                        std::map<std::string, std::string>(),
                        bool cla_flag = false)
{
    static std::vector<T> x_frame{size.xmin, size.xmin, size.xmin, size.xmax}, 
                   y_frame{size.ymin, size.ymin, size.ymax, size.ymax}, 
                   z_frame{size.zmax, size.zmin, size.zmin, size.zmin};

    x_frame.push_back(x);
    y_frame.push_back(y);
    z_frame.push_back(z);

    plt::plot3(x_frame, y_frame, z_frame, 1, kw);
    plt::pause(0.02);

    if(cla_flag)
    {
        plt::cla();
        x_frame.clear();
        y_frame.clear();
        z_frame.clear();
        x_frame = std::vector<T>{size.xmin, size.xmin, size.xmin, size.xmax};
        y_frame = std::vector<T>{size.ymin, size.ymin, size.ymax, size.ymax};
        z_frame = std::vector<T>{size.zmax, size.zmin, size.zmin, size.zmin};
        plt::plot3(x_frame, y_frame, z_frame, 1, kw);
        plt::pause(0.02);
    }
}

/* Functions -----------------------------------------------------------------*/
void test_gaitTraj()
{
    struct DesiredConstrain<float> test_constrain = {
        3, -3,
        3, -3,
        3 ,-3,
        3,
        3, -3,
        3, -3,
        3, -3,
        0.6, 0.1,
        0.01
    };

    Gait_Tester.initialize();
    Gait_Tester.SetPhaseInterval(0.013);
    Cmd_Tester.CoM_height = 0.7;
    Cmd_Tester.SetConstrains(test_constrain);
    Cmd_Tester.SetVelocityCommand(0.6f, 0.0f, 0.0f,10.0f);

    test_gaitTransition_one();
}

void test_2D_traj()
{
    Vec3<float> pFoot[4];
    Vec3<float> pFootStep[4];
    Vec3<float> velocity = Vec3<float>::Zero();

    for(int leg(0); leg < 4; leg ++)
    {
        pFootStep[leg] = 0.5f * Gait_Tester.GetPeriod() * Cmd_Tester.v_d
                        + sqrt(Cmd_Tester.CoM_height / CONST_GRAVITY) * (velocity - Cmd_Tester.v_d);

        pFoot[leg] = -pFootStep[leg];
        Gait_Tester.SetFootPlacement(pFoot[leg], pFootStep[leg], 0.2, leg);
    }

    /*
        Figure configuration.
    */
    float x[2],y[2],z[2];
    plt::xlim(-0.9, 0.9);
    plt::ylim(-0.05, 0.3);
    plt::title("Figure:Foot trajectory");
    plt::legend();

    /*
        下面用于显示平面图中的动态轨迹。
    */
    while (1)
    {
        for(int leg(0); leg < 4; leg ++)
        {
            // if(Gait_Tester.GetPeriodEndFlag(leg) || Gait_Tester.GetSwingEndFlag(leg))
            // {
            //     pFootStep[leg] = 0.5f * Gait_Tester.GetPeriod() * Cmd_Tester.v_d
            //                     + sqrt(Cmd_Tester.CoM_height/ CONST_GRAVITY) *
            //                     (velocity - Cmd_Tester.v_d);

            //     pFoot[leg] = -pFootStep[leg];
            //     Gait_Tester.SetFootPlacement(pFoot[leg], pFootStep[leg], 0.2, leg);

            //     velocity = Cmd_Tester.v_d;
            // }
        }

        Gait_Tester.gaitGenerate();
        x[0] = x[1];
        y[0] = y[1];
        z[0] = z[1];
        x[1] = Gait_Tester.GetFootPos(LegsName::RF)[0];
        y[1] = Gait_Tester.GetFootPos(LegsName::RF)[1];
        z[1] = Gait_Tester.GetFootPos(LegsName::RF)[2];

        plt::plot(x,z,"b-");
        plt::pause(0.02);

        //if(!Gait_Tester.GetFootState(LegsName::RF))
        if(Gait_Tester.GetFootState(LegsName::RF))
        {
            plt::cla();
            plt::xlim(-0.9, 0.9);
            plt::ylim(-0.05, 0.33);
            plt::title("Figure:Foot trajectory");
            plt::legend();
        }
    }

}

void test_3D_traj()
{
    Vec3<float> pFoot[4];
    Vec3<float> pFootStep[4];
    Vec3<float> velocity = Vec3<float>::Zero();

    for(int leg(0); leg < 4; leg ++)
    {
        pFootStep[leg] = 0.5f * Gait_Tester.GetPeriod() * Cmd_Tester.v_d
                        + sqrt(Cmd_Tester.CoM_height/ CONST_GRAVITY) *
                        (velocity - Cmd_Tester.v_d);

        pFoot[leg] = -pFootStep[leg];
        Gait_Tester.SetFootPlacement(pFoot[leg], pFootStep[leg], 0.2, leg);
    }
    /*
        Figure configuration.
    */
    std::vector<float> x,y,z;
    std::map<std::string, std::string> keywords;
    keywords.insert(std::pair<std::string, std::string>("label", "Walk:Sinusoidal - 0") );
    keywords.insert(std::pair<std::string, std::string>("linewidth", "1.5") );
    keywords.insert(std::pair<std::string, std::string>("color", "blue") );  

    /*
        - 下面用于显示三维图中的动态轨迹，轨触发overflow的flag后清除vector和图片。
        - pause时间建议是0.2。
    */
    while (1)
    {

        for(int leg(0); leg < 4; leg ++)
        {
            // if(Gait_Tester.GetPeriodEndFlag(leg) || Gait_Tester.GetSwingEndFlag(leg))
            // {
            //     pFootStep[leg] = 0.5f * Gait_Tester.GetPeriod() * Cmd_Tester.v_d
            //                     + sqrt(Cmd_Tester.CoM_height/ CONST_GRAVITY) *
            //                     (velocity - Cmd_Tester.v_d);

            //     pFoot[leg] = -pFootStep[leg];
            //     Gait_Tester.SetFootPlacement(pFoot[leg], pFootStep[leg], 0.2, leg);

            //     velocity = Cmd_Tester.v_d;
            // }
        }

        if(Gait_Tester.GetSwingEndFlag(LegsName::LF))
        {   
            x.clear();
            y.clear();
            z.clear();

            plt::cla();
            plt::plot3(x, y, z, 1, keywords);
            plt::title("Foot trajectory");
            plt::pause(0.02);
        }
        
        Gait_Tester.gaitGenerate();
        x.push_back(Gait_Tester.GetFootPos(LegsName::LF)[0]);
        y.push_back(Gait_Tester.GetFootPos(LegsName::LF)[1]);
        z.push_back(Gait_Tester.GetFootPos(LegsName::LF)[2]);

        plt::plot3(x, y, z, 1, keywords);
        plt::title("Foot trajectory");
        plt::pause(0.02);

    }
}

void test_gaitTransition_one()
{   
    int IRQsignal=1;
    /*
        Figure configuration.
    */
    std::vector<float> x,y,z;
    std::map<std::string, std::string> keywords;
    keywords.insert(std::pair<std::string, std::string>("linewidth", "1.5") );
    keywords.insert(std::pair<std::string, std::string>("color", "blue") );  

    Gait_Tester.initialize();
    Gait_Tester.SetPhaseInterval(0.05);
    float a=0.05;
    float walk_delta=0.0;
    while (1)
    {
        /*使用说明
        1.Gait_Tester.intialize().
        2.给STAND步态输入起点终点  
        比如：if(Gait_Tester.GetNextGaitType()==GaitType::STAND_UP)
                //TO DO:setfootplacement();
        3.Gait_Tester.gaitGenerate();
        4.Gait_Tester.changeGait(GaitType::WALK).
        5.给WALK步态输入起点终点
        比如：if(Gait_Tester.GetNextGaitType()==GaitType::WALK)
                //TO DO:setfootplacement();
        
        步态转换结束后current_gait=next_gait=WALK
        6.此时还想转TROT：Gait_Tester.changeGait(GaitType::TROT).
        给TROT步态输入起点终点
        比如：if(Gait_Tester.GetNextGaitType()==GaitType::WALK)
                //TO DO:setfootplacement();

        #-ATTENTION-#
        起点和终点可以在同一个函数给(不需要像下面的example那样分步态给点)，
        但是需要在步态转换的时候做一些处理(比如对速度加快，给点幅度要大一点；反之同理)。
        */
        //给stand up 的输入
        //GetNextGaitType()=stand 的意思是当前没有计划更改步态，将来也是stand  或者  当前不是步态过渡状态
        //注释：changeGait(walk)调用后使得next_gait=walk 而当前current_gait=STAND（因为没有转换结束）
        //转换结束后current_gait=next_gait=WALK
        if(Gait_Tester.GetNextGaitType()==GaitType::STAND_UP)
        {
            Vec3<float> startPos;//the position after finishing standing up.
            if (Gait_Tester.GetCurrentGaitType()==GaitType::STAND_UP)
            {
                walk_delta+=0.01;
            }
            
            //startPos<<0.19, 0.013, 0.25;
        if(IRQsignal)
        {   IRQsignal=0;
            startPos<<0.0,0.0,0.0;
        }
            Vec3<float> finalPos;//the position before executing standing up.
            finalPos<<0.0,0+walk_delta,0.5+walk_delta;
            //finalPos<<0,0,0;
            for (int id = 0; id < 4; id++)
            {
                Gait_Tester.SetFootPlacement(startPos,finalPos,0.0,id);
            }
        }

        Gait_Tester.gaitGenerate();
    
    // //循环转步态，切换之后再切换，最后在TROT  WALK 之间循环
    // if (Gait_Tester.GetCurrentGaitType()==GaitType::STAND_UP)
    // {
    //     Gait_Tester.changeGait(GaitType::TROT);
    // }
    // else if (Gait_Tester.GetCurrentGaitType()==GaitType::TROT)
    // {
    //     Gait_Tester.changeGait(GaitType::WALK);
    // }else if (Gait_Tester.GetCurrentGaitType()==GaitType::WALK)
    // {
    //     Gait_Tester.changeGait(GaitType::TROT);
    // }
    //给trot的输入
    if(Gait_Tester.GetNextGaitType()==GaitType::TROT)
    {   
        if(Gait_Tester.GetFootState(LegsName::RF)==1)
        {a+=0.01;}
        Vec3<float> startPos;//the position after finishing standing up.
        startPos<<0,0,0;
        
        Vec3<float> finalPos;//the position before executing standing up.
        finalPos<<1+a,1+a,0.0;
            for (int id = 0; id < 4; id++)
            {
                Gait_Tester.SetFootPlacement(startPos,finalPos,0.5,id);
            }
    }

    //给walk的输入
    if(Gait_Tester.GetNextGaitType()==GaitType::WALK)
    {
        if(Gait_Tester.GetFootState(LegsName::RF)==1)
        {a-=0.01;}
        Vec3<float> startPos;//the position after finishing standing up.
        startPos<<0,0,0;
        Vec3<float> finalPos;//the position before executing standing up.
        finalPos<<0.5+a,0.5+a,0.0;
        for (int id = 0; id < 4; id++)
        {
            Gait_Tester.SetFootPlacement(startPos,finalPos,0.5,id);
        }
    }
    
        x.push_back(Gait_Tester.GetFootPos(LegsName::RF)[0]);
        y.push_back(Gait_Tester.GetFootPos(LegsName::RF)[1]);
        z.push_back(Gait_Tester.GetFootPos(LegsName::RF)[2]);

        plt::plot3(x, y, z, 1, keywords);
        plt::title("gait transition trajectory");
        plt::pause(0.02);

        if(0)
        {   
            x.clear();
            y.clear();
            z.clear();

            plt::cla();
            plt::plot3(x, y, z, 1, keywords);
            plt::title("Foot trajectory");
            plt::pause(0.02);
        }
    }
}

void test_gaitTransition_two()
{
    int IRQsignal=1;

    std::vector<float> x,y,z;
    std::map<std::string, std::string> keywords;
    keywords.insert(std::pair<std::string, std::string>("linewidth", "1.5") );
    keywords.insert(std::pair<std::string, std::string>("color", "blue") );  

    Gait_Tester.initialize();
    Gait_Tester.SetPhaseInterval(0.01);

while (1)
{
    while (1)
    {
         if (Gait_Tester.GetNextGaitType()==GaitType::STAND_UP)
        {
            for (int id = 0; id < 4; id++)
            {
                Vec3<float> startpos;
                startpos<<0,0,0.5;
                Vec3<float> finalpos;
                finalpos<<0,0,0;
                Gait_Tester.SetFootPlacement(startpos,finalpos,0.0,id);
            }
        }
    Gait_Tester.gaitGenerate();
    x.push_back(Gait_Tester.GetFootPos(LegsName::RF)[0]);
    y.push_back(Gait_Tester.GetFootPos(LegsName::RF)[1]);
    z.push_back(Gait_Tester.GetFootPos(LegsName::RF)[2]);

    plt::plot3(x, y, z, 1, keywords);
    plt::title("gait transition trajectory");
    plt::pause(0.02);
        if (Gait_Tester.GetExeStanceFlag()==1)
        {
            break;
        }
    }
    
     Gait_Tester.changeGait(GaitType::TROT);

while (1)
{
   if (Gait_Tester.GetNextGaitType()==GaitType::TROT)
    {
        for (int i = 0; i < 4; i++)
        {
            Vec3<float> startpos;
            startpos<<0,0,0;
            Vec3<float> finalpos;
            finalpos<<1.0,1.0,0;
            Gait_Tester.SetFootPlacement(startpos,finalpos,0.4,i);
        } 
    }
    Gait_Tester.gaitGenerate();
    x.push_back(Gait_Tester.GetFootPos(LegsName::RF)[0]);
    y.push_back(Gait_Tester.GetFootPos(LegsName::RF)[1]);
    z.push_back(Gait_Tester.GetFootPos(LegsName::RF)[2]);

    plt::plot3(x, y, z, 1, keywords);
    plt::title("gait transition trajectory");
    plt::pause(0.02);

    
}
}

}

/************************* END-OF-FILE SCUT-ROBOTLAB **************************/